using PathX.Collection;
using System.Collections.Generic;

namespace PathX
{
    public class AStarPathingEngine : PathingEngineBase
    {
        private BinaryHeap<IPathNode> _openSet;
        private DynamicArray<IPathNode> _successorArray;
        private IPathNode _closestNode;
        private bool _preventDiagonalMoves;
        private IMoveCost _moveCost;

        protected List<IPathNode> _expandedSet;
        protected IPathNode _current;

        /// <summary>
        /// The attributes of the path requester
        /// </summary>
        protected IUnitProperties _unitProps;

        /// <summary>
        /// The unit's attributes
        /// </summary>
        protected AttributeMask _unitAttributes;
        
        
        public AStarPathingEngine(int heapInitialSize) : base(new ManhattanDistance(10))
        {
            _openSet = new BinaryHeap<IPathNode>(heapInitialSize, new PathNodeComparer());
            _expandedSet = new List<IPathNode>();
            _successorArray = new DynamicArray<IPathNode>(15);
        }

        /// <summary>
        /// Gets the walkable successors of the specified node.
        /// </summary>
        /// <param name="current">The current node.</param>
        /// <param name="successorArray">The array to fill with successors.</param>
        /// <returns>All walkable successors of the node.</returns>
        private void GetWalkableSuccessors(IPathNode current, DynamicArray<IPathNode> successorArray)
        {
            current.GetWalkableNeighbours(successorArray, _unitProps, false, _preventDiagonalMoves);
        }

        /// <summary>
        /// Called when a request is about to be processed.
        /// </summary>
        /// <param name="start">The start node.</param>
        /// <param name="actualStart">The actual start node in case actual start is blocked.</param>
        protected override void OnStart(IPathNode start, IPathNode actualStart)
        {
            _openSet.Clear();

            //Reset all g's on all nodes marking them not expanded
            _expandedSet.ForEach(c => c.g = 0);
            _expandedSet.Clear();
            _closestNode = start;

            //Initialize and add the start node. Since no expanded set is used start g must be initialized to a small > 0  value to mark it as expanded
            //Since the start node will never reenter the open set, there is no need to initialize the h and f values.
            start.g = 1;
            start.h = GetHeuristic(start, this.goal);
            start.predecessor = actualStart;

            _openSet.Add(start);
            _expandedSet.Add(start);

            if (actualStart != null)
            {
                actualStart.g = 1;
                actualStart.predecessor = null;
                _expandedSet.Add(actualStart);
            }

            _unitProps = this.currentRequest.requesterProperties;
            _unitAttributes = _unitProps.attributes;
            _preventDiagonalMoves = this.currentRequest.pathFinderOptions.preventDiagonalMoves;
        }

        /// <summary>
        /// Processes the next node.
        /// </summary>
        /// <returns>The current pathing status</returns>
        protected override PathingStatus ProcessNext()
        {
            //First we need to explore the various exit conditions
            if (!_openSet.hasNext)
            {
                if (this.currentRequest.pathFinderOptions.navigateToNearestIfBlocked)
                {
                    UpdateGoal(_closestNode);
                    return PathingStatus.Complete;
                }

                if (!this.goal.IsWalkableWithClearance(_unitProps))
                {
                    //Goal became unwalkable during a star, which can happen in both synchronous and async operation, since synchronous can have part of its processing split across frames if the time threshold is hit.
                    return PathingStatus.DestinationBlocked;
                }

                //All routes have been explored without finding a route to destination, i.e. one does not (currently) exist
                return PathingStatus.NoRouteExists;
            }

            //I doubt there is any reason to have this added complexity that the user needs to consider, just resize
            if (_openSet.count == _openSet.capacity)
            {
                _openSet.Resize();
            }

            _current = _openSet.Remove();
            if (_current == this.goal)
            {
                //Hurray we found a route
                return PathingStatus.Complete;
            }

            if (_closestNode.h > _current.h)
            {
                _closestNode = _current;
            }

            _current.isClosed = true;

            //Get potential successors
            _successorArray.Clear();
            GetWalkableSuccessors(_current, _successorArray);

            //Assign costs
            var walkableCount = _successorArray.count;
            for (int i = 0; i < walkableCount; i++)
            {
                var n = _successorArray[i];
                if (n == null)
                {
                    break;
                }
                
                var cost = _current.g + n.cost;
                
                cost += this.costProvider.GetMoveCost(_current, n);
                

                //Instead of operating with an expanded set (closed set + open set) we simply evaluate the g value of the node, since g will only ever be > 0 if the node has been expanded.
                //This saves lots of memory over time and is even more O(1) than checking a hash set
                if (n.g > 0)
                {
                    //In the case that the neighbour has been evaluated in context to another node but gets a better cost in relation the current node, update its state.
                    if (n.g > cost)
                    {
                        n.g = cost;
                        n.f = cost + n.h;
                        n.predecessor = _current;

                        //If the node is closed, i.e. was removed from the open set, open it again. Otherwise update its position in the open set.
                        if (n.isClosed)
                        {
                            n.isClosed = false;
                            _openSet.Add(n);
                        }
                        else
                        {
                            _openSet.ReheapifyUpFrom(n);
                        }
                    }
                }
                else
                {
                    //Add the node to the open set
                    n.g = cost;
                    n.h = GetHeuristic(n, _current);
                    n.f = cost + n.h;
                    n.predecessor = _current;
                    n.isClosed = false;

                    _openSet.Add(n);
                    _expandedSet.Add(n);
                }
            }

            return PathingStatus.Running;
        }

        private int GetHeuristic(IPathNode n, IPathNode current)
        {
            var best = this.costProvider.GetHeuristic(n, this.goal);

            return best;
        }
    }
}